#! /usr/bin/env python

PACKAGE='dvo_ros'
import os
import roslib; roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator import *

me = os.path.realpath(__file__)
cpp_header = os.path.realpath(os.path.join(os.path.dirname(me), "cpp", "dvo_ros", "CameraDenseTrackerConfig.h"))

if os.path.exists(cpp_header) and os.path.getmtime(me) < os.path.getmtime(cpp_header):
    exit(0)

gen = ParameterGenerator()

RUN_DENSE_TRACKING = gen.const("RunDenseTracking", int_t, 0x00000001, "run_dense_tracking")
USE_DENSE_TRACKING_ESTIMATE = gen.const("UseDenseTrackingEstimate", int_t, 0x00000002, "use_dense_tracking_estimate")
CONFIG_PARAM = gen.const("ConfigParam", int_t, 0x00000004, "config_param")
MISC_PARAM = gen.const("MiscParam", int_t, 0x00000008, "misc_param")

#       Name, Type, Reconfiguration level, Description, Default, Min, Max
gen.add("run_dense_tracking", bool_t, RUN_DENSE_TRACKING["value"], "", False)
gen.add("use_dense_tracking_estimate", bool_t, USE_DENSE_TRACKING_ESTIMATE["value"], "", False)

scale_estimator_enum = gen.enum([ 
    gen.const("NormalDistributionScaleEstimator", int_t, 1, ""),
    gen.const("TDistributionScaleEstimator", int_t, 2, ""),
    gen.const("MADScaleEstimator", int_t, 3, "") 
], "scale estimator")


influence_function_enum = gen.enum([ 
    gen.const("TukeyInfluenceFunction", int_t, 1, ""),
    gen.const("TDistributionInfluenceFunction", int_t, 2, ""),
    gen.const("HuberInfluenceFunction", int_t, 3, ""),
], "influence function")

# DenseTracker::Config

gen.add("coarsest_level",			int_t,		CONFIG_PARAM["value"], "", 3,		0, 5	)
gen.add("finest_level",				int_t,		CONFIG_PARAM["value"], "", 1,		0, 5	)
gen.add("max_iterations",			int_t,		CONFIG_PARAM["value"], "", 50,		1, 500	)
gen.add("precision",				double_t,	CONFIG_PARAM["value"], "", 0.0001,	0, 1	)
gen.add("use_initial_estimate",		bool_t,		CONFIG_PARAM["value"], "", True				)
gen.add("use_weighting",            bool_t,     CONFIG_PARAM["value"], "", False             )
gen.add("scale_estimator",          int_t,      CONFIG_PARAM["value"], "", 2,        0, 20,  edit_method = scale_estimator_enum)
gen.add("scale_estimator_param",    double_t,   CONFIG_PARAM["value"], "", 5.0,      0, 100  )
gen.add("influence_function",       int_t,      CONFIG_PARAM["value"], "", 2,        0, 20,  edit_method = influence_function_enum)
gen.add("influence_function_param", double_t,   CONFIG_PARAM["value"], "", 5.0,      0, 100  )
gen.add("min_intensity_deriv",   double_t,   CONFIG_PARAM["value"], "", 0,		0, 255	)
gen.add("min_depth_deriv",       double_t,   CONFIG_PARAM["value"], "", 0,		0, 10	)
gen.add("lambda",					double_t,   CONFIG_PARAM["value"], "", 0,		0, 10	)
gen.add("mu",						double_t,   CONFIG_PARAM["value"], "", 0,		0, 10	)

gen.add("reconstruction",           bool_t,     MISC_PARAM["value"],   "", True    )

exit(gen.generate(PACKAGE, "dvo", "CameraDenseTracker"))
